#include "KalmanFiltering.h"
#include <iostream>

void KalmanFiltering::InitStateX(Eigen::VectorXd & inputX) {
    X_Hat = Eigen::VectorXd(inputX);
    stateDimension = inputX.size();
    I =  Eigen::MatrixXd::Identity(stateDimension, stateDimension);
    Kk = Eigen::MatrixXd(stateDimension, stateDimension);
    isInit = true;
}


void KalmanFiltering::Init(Eigen::MatrixXd & p0, Eigen::MatrixXd & inputR, Eigen::MatrixXd & inputH) {
    P = Eigen::MatrixXd(p0);
    R = Eigen::MatrixXd(inputR);
    H = Eigen::MatrixXd(inputH);
}

void KalmanFiltering::SetA(Eigen::MatrixXd & inputA) {
    A = Eigen::MatrixXd(inputA);
}

void KalmanFiltering::SetQ(Eigen::MatrixXd &inputQ) {
    Q = Eigen::MatrixXd(inputQ);
}

void KalmanFiltering::Predict() {
    X_Prior_Hat = A * X_Hat;
    P_Prior = A * P * A.transpose() + Q;
//    X1_Prior_Hat.push_back(X_Prior_Hat(0,0));
//    X2_Prior_Hat.push_back(X_Prior_Hat(1,0));
}

void KalmanFiltering::Correct(Eigen::MatrixXd & inputZ) {
    Kk = P_Prior * H.transpose() * ((H * P_Prior * H.transpose() + R).inverse());
    X_Hat = X_Prior_Hat + Kk * (inputZ - H * X_Prior_Hat);
//    X1_Hat.push_back(X_Hat(0,0));
//    X2_Hat.push_back(X_Hat(1,0));
}

void KalmanFiltering::UpdateP() {
    P = (I - Kk * H) * P_Prior;
}